﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;
using Drifting.MotionPlanning.Systems.CollisionPrimitives;

namespace Drifting.MotionPlanning.Systems
{
    public class PlanarEnvironment : BaseSystem
    {
        List<Polygon2D> robot;
        List<Polygon2D> obstacles;

        public PlanarEnvironment(string environment, string system)
            : base(system)
        {

            if (environment != null)
            {
                string rawText = TextAssistant.LoadFile(environment);

                string[] separators = null;
                string[] entries = null;

                separators = new string[] { "<robot>", "</robot>" };
                entries = rawText.Split(separators, StringSplitOptions.None);
                
                int n_parts = (entries.Length - 1) / 2;
                robot = new List<Polygon2D>(n_parts);
                for (int i = 0; i < n_parts; ++i)
                {
                    Polygon2D p = new Polygon2D(entries[1 + i * 2]);
                    robot.Add(p);
                }

                separators = new string[] { "<obstacle>", "</obstacle>" };
                entries = rawText.Split(separators, StringSplitOptions.None);

                n_parts = (entries.Length - 1) / 2;
                obstacles = new List<Polygon2D>(n_parts);
                for (int i = 0; i < n_parts; ++i)
                {
                    Polygon2D p = new Polygon2D(entries[1 + i * 2]);
                    obstacles.Add(p);
                }
            }
        }

        public List<Polygon2D> Robot
        {
            get { return robot; }
        }

        public List<Polygon2D> Obstacles
        {
            get { return obstacles; }
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            throw new NotImplementedException();
        }

        public bool CollisionFree(double[] q)
        {         
            foreach(Polygon2D robotPart in robot)
            {
                foreach (Polygon2D obstacle in obstacles)
                {
                    if (robotPart.IsCollidedWithIfAt(q, obstacle))
                        return false;
                }
            }

            return true;
        }
    }
}
